#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
本程序的运行逻辑：
无人机自行起飞--等待服务器端输入坐标--根据坐标执行飞行任务（单次）--降落
© Copyright 2015-2016, 3D Robotics.
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto.
"""

from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import re
import serial
import numpy as np
import matplotlib.pyplot as plt
import socket

global count_get_uwb  # 只执行一次
global plot_x
global plot_y  # 画图用全局变量坐标
global x
global y
count_get_uwb = 0
plot_x = []
plot_y = []
x = 0
y = 0
#初始化局域网通信
HOST = '192.168.137.59'           #服务器的ip
PORT = 6667                    #服务器端口
sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)  #以TCP协议模式进行通信
sock.connect((HOST,PORT))         #连接服务器


# 连接的pixhawk飞控的端口
connection_string = '/dev/ttyACM0'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect(connection_string, wait_ready=True, baud=921600)

def plot_pos(A,B):  # 绘制飞行轨迹
    global plot_x
    global plot_y
    plt.clf()
    plot_x.append(A)
    plot_y.append(B)
    plt.figure(figsize=(5, 10))
    plt.plot(plot_x, plot_y)
    plt.scatter(plot_x, plot_y, s=10, c='red')
    plt.xlim(0, 3)
    plt.ylim(0, 6)
    plt.xticks(range(0, 3, 1))
    plt.yticks(range(0, 6, 1))
    plt.xlabel("Y")
    plt.ylabel("X")
    plt.show()
# 绘图


def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,  # time_boot_ms (not used)
        0, 0,  # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED,  # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
        0b0000111111000111,  # type_mask
        0, 0, 0,  # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z,  # m/s
        0, 0, 0,  # x, y, z acceleration
        0, 0)
    for x in range(0, duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
# 参数aTargetAltitude即为目标高度，单位为米
def arm_and_takeoff(aTargetAltitude):
    #     if(abs(vehicle.location.global_relative_frame.alt)>0.5):
    #         print("Alt error.Arming failed",vehicle.location.global_relative_frame.alt)
    #         vehicle.close()
    #         return
    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True
    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    # 发送起飞指令
    print("Taking off!")
    vehicle.simple_takeoff(1)
    time.sleep(3)
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
    #     vehicle.simple_takeoff(aTargetAltitude)

    # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print("Rangefinder distance:", vehicle.rangefinder.distance)
        if vehicle.rangefinder.distance >= 1 * 0.95:
            print("Reached target altitude")
            time.sleep(2)
            break
        else:
            send_body_ned_velocity(0, 0, -0.1, 1)
            print("UP 0.1m")
            time.sleep(1)


# 调用上面声明的arm_and_takeoff函数，目标高度1m
temp_alt = vehicle.location.global_relative_frame.alt
print("Inti alt is", temp_alt)
arm_and_takeoff(1)


# 定义方向控制


# #也可以用下面这种格式:让无人机向右飞行,速度0.2m/s,飞行时间5秒，延迟(sleep)2秒
# send_body_ned_velocity(0,0.2,0,5)
# time.sleep(2)

def get_uwb():
    global plot_x
    global plot_y
    global count_get_uwb
    global x
    global y
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)  # change tty/baud rate/timeout here
    #     temp_x=0
    #     temp_y=0
    while True:
        line = ser.readline()  # read position info
        line = str(line)  # byte->str
        if line:  # if accepted
            a = re.search('KT', line)  # find xyz information
            b = re.search('NULL', line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b:
                    #                 print(type(line))
                    count = line.split(',', 7)  # standard
                    #                     print ("count=",count)
                    temp1 = count[5].split('(')
                    temp2 = count[7].split(')')
                    position = [0, 0, 0]
                    position[0] = temp1[1]
                    position[1] = count[6]
                    position[2] = temp2[0]
                    temp_x = x  # 上一轮次收到的位置信息
                    temp_y = y
                    x = float(position[1])  # 本轮收到的位置信息
                    y = float(position[0])
                    if count_get_uwb == 0:
                        temp_x = x
                        temp_y = y
                        count_get_uwb = 1
                    s1 = x - temp_x  # 计算两次差值
                    s2 = y - temp_y
                    if (abs(s1) > 0.3):  # 如果差值大于0.3进行修正
                        x = temp_x + np.sign(s1) * 0.3
                        print("Value [X] is corrected")
                    if (abs(s2) > 0.3):
                        y = temp_y + np.sign(s2) * 0.3
                        print("Value [Y] is corrected")
                    print("Camera position", x, y)
                    plot_pos(x,y)
                    break
        else:
            break
    return [x, y]


# 以下是坐标飞行模式
def goto_pos(goal):
    print("Start Mission. Goal is:", goal)
    while True:
        ori = get_uwb()  # 当前坐标点
        print("ori", ori)
        actx = goal[0] - ori[0]  # 距离目标的绝对距离
        acty = goal[1] - ori[1]  # 距离目标的绝对距离
        if (actx > 0.15):  # 判断前进方向
            flag = 1  # 正飞
        elif (abs(actx) < 0.15):
            flag = 0  #
        elif (actx < -0.15):
            flag = -1  # 反飞
        if (acty > 0.15):  # 判断前进方向
            flag2 = 0.5  # 正飞
            if (acty > 0.4):
                flag2 = 1
        elif (abs(acty) < 0.15):
            flag2 = 0  #
        elif (acty < -0.15):
            flag2 = -0.5  # 反飞
            if (acty < -0.4):
                flag2 = -1
        # print("actx:",actx)
        # print("acty:",acty)
        send_body_ned_velocity(flag * 0.1, flag2 * 0.1, 0, 1)
        if (flag == 0 and flag2 == 0):  # 距离目标点误差为0.1m以内就可以停止
            break
        time.sleep(1)
    print("ARIVIED")
    final = get_uwb()
    time.sleep(1)


#以下是多坐标点飞行模式
def path_goto(path_array):
    for i in path_array:
        goto_pos(i)

plt.ion()

def monitor_goto():
    print("Waiting for position information from server")
    while True:
        x=0
        y=0
        data=sock.recv(1024)
        data1 = data.decode()
        count = data1.split(',', 2)  # standard
        print(count)
        x=float(count[0])
        y=float(count[1])    
        print("x:",x,"y:",y)
        plot_pos(x,y)
        time.sleep(1)
        if (x!=0 and y!=0):
            destation=[[x,y]]#收到坐标
            path_goto(destation)
            sock.close()#关闭连接
            return#仅接受一个点


#执行服务器给予的点飞行
monitor_goto()



#以下是手动给予坐标并且让其沿着坐标依次飞行
# destation = [[3.2, 1.6]]
# path_goto(destation)



# 发送"降落"指令
print("Land")
# 降落，只需将无人机的飞行模式切换成"Land"
vehicle.mode = VehicleMode("LAND")


plt.close()#注释此行可以让程序结束运行后仍然保持图像
# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()
